/***************************************************************************
 *   Copyleft 2006-2007 by												   *
 *	 Iwanicki Krzysztof													   *
 *   <gamma.mini.pw.edu.pl/~iwanickik>									   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 ***************************************************************************/
#include "./Matrix4D.h"
#include "../../Macros/Macros.h"


/**************************************************************
* Constructors                                                *
***************************************************************/
Matrix4D::Matrix4D() {
	m11 = 1.0f;
	m22 = 1.0f;
	m33 = 1.0f;
	m44 = 1.0f;

	m12 = m13 = m14 = 0.0f;
	m21 = m23 = m24 = 0.0f;
	m31 = m32 = m34 = 0.0f;
	m41 = m42 = m43 = 0.0f;
}

Matrix4D::Matrix4D(cFloat* tab) {
	for (cInt iCnt = 0; iCnt < 16; iCnt++) {
		mValues[iCnt] = tab[iCnt];
	}
}

Matrix4D::Matrix4D(const Matrix4D& matrix) {
	for (cInt iCnt = 0; iCnt < 16; iCnt++) {
		mValues[iCnt] = matrix.mValues[iCnt];
	}
}

/**************************************************************
* Operators                                                   *
***************************************************************/
Matrix4D Matrix4D::operator*(const Matrix4D& b) {
	Matrix4D matrix;

	matrix.m11 = m11 * b.m11 + m12 * b.m21 + m13 * b.m31 + m14 * b.m41;
	matrix.m12 = m11 * b.m12 + m12 * b.m22 + m13 * b.m32 + m14 * b.m42;
	matrix.m13 = m11 * b.m13 + m12 * b.m23 + m13 * b.m33 + m14 * b.m43;
	matrix.m14 = m11 * b.m14 + m12 * b.m24 + m13 * b.m34 + m14 * b.m44;

	matrix.m21 = m21 * b.m11 + m22 * b.m21 + m23 * b.m31 + m24 * b.m41;
	matrix.m22 = m21 * b.m12 + m22 * b.m22 + m23 * b.m32 + m24 * b.m42;
	matrix.m23 = m21 * b.m13 + m22 * b.m23 + m23 * b.m33 + m24 * b.m43;
	matrix.m24 = m21 * b.m14 + m22 * b.m24 + m23 * b.m34 + m24 * b.m44;

	matrix.m31 = m31 * b.m11 + m32 * b.m21 + m33 * b.m31 + m34 * b.m41;
	matrix.m32 = m31 * b.m12 + m32 * b.m22 + m33 * b.m32 + m34 * b.m42;
	matrix.m33 = m31 * b.m13 + m32 * b.m23 + m33 * b.m33 + m34 * b.m43;
	matrix.m34 = m31 * b.m14 + m32 * b.m24 + m33 * b.m34 + m34 * b.m44;

	matrix.m41 = m41 * b.m11 + m42 * b.m21 + m43 * b.m31 + m44 * b.m41;
	matrix.m42 = m41 * b.m12 + m42 * b.m22 + m43 * b.m32 + m44 * b.m42;
	matrix.m43 = m41 * b.m13 + m42 * b.m23 + m43 * b.m33 + m44 * b.m43;
	matrix.m44 = m41 * b.m14 + m42 * b.m24 + m43 * b.m34 + m44 * b.m44;

	return matrix;
}

Point4D Matrix4D::operator*(const Point4D& b) {
	Point4D point;

	point.mX = b.mX * m11 + b.mY * m12 + b.mZ * m13 + b.mW * m14;
	point.mY = b.mX * m21 + b.mY * m22 + b.mZ * m23 + b.mW * m24;
	point.mZ = b.mX * m31 + b.mY * m32 + b.mZ * m33 + b.mW * m34;
	point.mW = b.mX * m41 + b.mY * m42 + b.mZ * m43 + b.mW * m44;

	return point;
}

Matrix4D Matrix4D::operator*(const cFloat fFactor) {
	Matrix4D matrix;
	
	for (cInt i = 0; i < 16; i++) {
		matrix.mValues [i] = mValues[i] * fFactor;
	}

	return matrix;
}

Matrix4D& Matrix4D::operator=(const Matrix4D &matrix) {
	for (cInt iCnt = 0; iCnt < 16; iCnt++) {
		mValues[iCnt] = matrix.mValues[iCnt];
	}

	return *this;
}

cFloat& Matrix4D::operator()(cInt row, cInt col) {
	return mValues[(row - 1) * 4 + (col - 1)];
}

/**************************************************************
* Other helpful functions                                     *
***************************************************************/

/**************************************************************
* Determinant calculations from OGRE Engine http://ogre3d.org *
* It's fast and effective, used for inverting				  *
***************************************************************/
cFloat Matrix4D::Determinant() const {
	return (m11 * (m22 * (m33 * m44 - m43 * m34) - m23 * (m32 * m44 - m42 * m34) + m24 * (m32 * m43 - m42 * m33))
			- m12 * (m21 * (m33 * m44 - m43 * m34) - m23 * (m31 * m44 - m41 * m34) + m24 * (m31 * m43 - m41 * m33))
			+ m13 * (m21 * (m32 * m44 - m42 * m34) - m22 * (m31 * m44 - m41 * m34) + m24 * (m31 * m42 - m41 * m32))
			- m14 * (m21 * (m32 * m43 - m42 * m33) - m22 * (m31 * m43 - m41 * m33) + m23 * (m31 * m42 - m41 * m32)));
}
/**************************************************************
* Inverse matrix calculations from OGRE Engine                *
* http://ogre3d.org											  *
***************************************************************/
Matrix4D Matrix4D::Invert() {
	Matrix4D matrix;

	matrix.m11 = m22 * (m33 * m44 - m43 * m34) - m23 * (m32 * m44 - m42 * m34) + m24 * (m32 * m43 - m42 * m33);
	matrix.m12 = -(m12 * (m33 * m44 - m43 * m34) - m13 * (m32 * m44 - m42 * m34) + m14 * (m32 * m43 - m42 * m33));
	matrix.m13 = m12 * (m23 * m44 - m43 * m24) - m13 * (m22 * m44 - m42 * m24) + m14 * (m22 * m43 - m42 * m23);
	matrix.m14 = -(m12 * (m23 * m34 - m33 * m24) - m13 * (m22 * m34 - m32 * m24) + m14 * (m22 * m33 - m32 * m23));

	matrix.m21 = -(m21 * (m33 * m44 - m43 * m34) - m23 * (m31 * m44 - m41 * m34) + m24 * (m31 * m43 - m41 * m33));
	matrix.m22 = m11 * (m33 * m44 - m43 * m34) - m13 * (m31 * m44 - m41 * m34) + m14 * (m31 * m43 - m41 * m33);
	matrix.m23 = -(m11 * (m23 * m44 - m43 * m24) - m13 * (m21 * m44 - m41 * m24) + m14 * (m21 * m43 - m41 * m23));
	matrix.m24 = m11 * (m23 * m34 - m33 * m24) - m13 * (m21 * m34 - m31 * m24) + m14 * (m21 * m33 - m31 * m23);

	matrix.m31 = m21 * (m32 * m44 - m42 * m34) - m22 * (m31 * m44 - m41 * m34) + m24 * (m31 * m42 - m41 * m32);
	matrix.m32 = -(m11 * (m32 * m44 - m42 * m34) - m12 * (m31 * m44 - m41 * m34) + m14 * (m31 * m42 - m41 * m32));
	matrix.m33 = m11 * (m22 * m44 - m42 * m24) - m12 * (m21 * m44 - m41 * m24) + m14 * (m21 * m42 - m41 * m22);
	matrix.m34 = -(m11 * (m22 * m34 - m32 * m24) - m12 * (m21 * m34 - m31 * m24) + m14 * (m21 * m32 - m31 * m22));

	matrix.m41 = -(m21 * (m32 * m43 - m42 * m33) - m22 * (m31 * m43 - m41 * m33) + m23 * (m31 * m42 - m41 * m32));
	matrix.m42 = m11 * (m32 * m43 - m42 * m33) - m12 * (m31 * m43 - m41 * m33) + m13 * (m31 * m42 - m41 * m32);
	matrix.m43 = -(m11 * (m22 * m43 - m42 * m23) - m12 * (m21 * m43 - m41 * m23) + m13 * (m21 * m42 - m41 * m22));
	matrix.m44 = m11 * (m22 * m33 - m32 * m23) - m12 * (m21 * m33 - m31 * m23) + m13 * (m21 * m32 - m31 * m22);

	matrix = matrix * (1.0f / Determinant());

	return matrix;
}

Matrix4D Matrix4D::Transpose() {
	Matrix4D matrix;

	matrix.m11 = m11;
	matrix.m22 = m22;
	matrix.m33 = m33;
	matrix.m44 = m44;

	matrix.m12 = m21;
	matrix.m13 = m31;
	matrix.m14 = m41;

	matrix.m21 = m12;
	matrix.m23 = m32;
	matrix.m24 = m42;

	matrix.m31 = m13;
	matrix.m32 = m23;
	matrix.m34 = m43;

	matrix.m41 = m14;
	matrix.m42 = m24;
	matrix.m43 = m34;

	return matrix;
}

Matrix4D Matrix4D::Translation(const Point4D& point4d) {
	Matrix4D matrix;

	matrix.m14 = point4d.mX;
	matrix.m24 = point4d.mY;
	matrix.m34 = point4d.mZ;

	return matrix;
}

Matrix4D Matrix4D::RotationX(cFloat fAngle) {
	Matrix4D matrix;

	matrix.m22 = matrix.m33 = COS_FROM_ANGLE(fAngle);
	matrix.m32 = SIN_FROM_ANGLE(fAngle);
	matrix.m23 = -matrix.m32;

	return matrix;	
}

Matrix4D Matrix4D::RotationY(cFloat fAngle) {
	Matrix4D matrix;

	matrix.m11 = matrix.m33 = COS_FROM_ANGLE(fAngle);
	matrix.m13 = SIN_FROM_ANGLE(fAngle);
	matrix.m31 = -matrix.m13;

	return matrix;
}

Matrix4D Matrix4D::RotationZ(cFloat fAngle) {
	Matrix4D matrix;

	matrix.m11 = matrix.m22 = COS_FROM_ANGLE(fAngle);
	matrix.m21 = SIN_FROM_ANGLE(fAngle);
	matrix.m12 = -matrix.m21;

	return matrix;
}

Matrix4D Matrix4D::Scaling(cFloat fFactor) {
	Matrix4D matrix;

	matrix.m11 = matrix.m22 = matrix.m33 = fFactor;

	return matrix;
}

Matrix4D Matrix4D::Scaling(cFloat fXFactor, cFloat fYFactor, cFloat fZFactor) {
	Matrix4D matrix;

	matrix.m11 = fXFactor;
	matrix.m22 = fYFactor;
	matrix.m33 = fZFactor;

	return matrix;
}

Matrix4D Matrix4D::Perspective(cFloat fFov) {
	Matrix4D matrix;

	matrix.m43 = 1.0f / fFov;
	matrix.m33 = 0.0f;

	return matrix;
}

Matrix4D Matrix4D::PerspectiveLeftEye(cFloat fFov, cFloat fEyeDistance) {
	Matrix4D matrix;

	matrix.m43 = 1.0f / fFov;
	matrix.m13 = -fEyeDistance / (2.0f * fFov);
	matrix.m33 = 0.0f;

	return matrix;
}

Matrix4D Matrix4D::PerspectiveRightEye(cFloat fFov, cFloat fEyeDistance) {
	Matrix4D matrix;

	matrix.m43 = 1.0f / fFov;
	matrix.m13 = fEyeDistance / (2.0f * fFov);
	matrix.m33 = 0.0f;

	return matrix;
}

Matrix4D Matrix4D::Identity() {
	Matrix4D matrix;

	matrix.m11 = 1.0f;
	matrix.m22 = 1.0f;
	matrix.m33 = 1.0f;
	matrix.m44 = 1.0f;

	matrix.m12 = matrix.m13 = matrix.m14 = 0.0f;
	matrix.m21 = matrix.m23 = matrix.m24 = 0.0f;
	matrix.m31 = matrix.m32 = matrix.m34 = 0.0f;
	matrix.m41 = matrix.m42 = matrix.m43 = 0.0f;

	return matrix;
}

Matrix4D Matrix4D::Zeros() {
	Matrix4D matrix;


	for (cInt iCnt = 0; iCnt < 16; iCnt++) {
		matrix.mValues[iCnt] = 0.0f;
	}

	return matrix;
}

cVoid Matrix4D::ToString() {
	LOGS.Info("|%-6.8f %-6.8f %-6.8f %-6.8f|", m11, m12, m13, m14);
	LOGS.Info("|%-6.8f %-6.8f %-6.8f %-6.8f|", m21, m22, m23, m24);
	LOGS.Info("|%-6.8f %-6.8f %-6.8f %-6.8f|", m31, m32, m33, m34);
	LOGS.Info("|%-6.8f %-6.8f %-6.8f %-6.8f|", m41, m42, m43, m44);
}

/*virtual*/ TiXmlElement* Matrix4D::Serialize(TiXmlElement* parent) {
	cChar buf[MAX_BUF_SIZE];
	
	sprintf_s(buf, "%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f", m11, m12, m13, m14, 
		m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44);
	TiXmlText* elem = new TiXmlText(buf);
	parent->LinkEndChild(elem);

	return cNull;
}